/*
 * VirtualHardwareController.cpp
 *
 *  Created on: 13 dec. 2012
 *      Author: Admin
 */

#include "Navigator.hpp"
#include "Protocol.hpp"
#include "FollowColorLaneState.hpp"
#include "Robot.hpp"
#include "SimulatorServer.hpp"
#include "VirtualHardwareController.hpp"

#include <iostream>
#include <windows.h>

bool runRobot = 1;
bool restartRobot = 0;

HANDLE mutex = CreateMutex(
			NULL,              // default security attributes
			FALSE,             // initially not owned
			NULL),             // unnamed mutex
       robotThreadMutex = CreateMutex(
			NULL,
			FALSE,
			NULL);

void waitForRobotThreadMutex()
{
	switch (WaitForSingleObject(
				robotThreadMutex,    // handle to mutex
		            INFINITE))  // no time-out interval
		{
			case WAIT_OBJECT_0:
				break;
			case WAIT_ABANDONED:
				throw std::exception();
		}
}

void releaseRobotThreadMutex()
{
	if (! ReleaseMutex(robotThreadMutex))
	{
		throw std::exception();
	}
}

void waitForMutex()
{
	switch (WaitForSingleObject(
	            mutex,    // handle to mutex
	            INFINITE))  // no time-out interval
	{
		case WAIT_OBJECT_0:
			break;
		case WAIT_ABANDONED:
			throw std::exception();
	}
}

void releaseMutex()
{
	if (! ReleaseMutex(mutex))
	{
		throw std::exception();
	}
}

void startRobotThread()
{
	waitForRobotThreadMutex();
	runRobot = 1;
	releaseRobotThreadMutex();
}

void pauzeRobotThread()
{
	waitForRobotThreadMutex();
	runRobot = 0;
	releaseRobotThreadMutex();
}

void restartRobotThread()
{
	waitForRobotThreadMutex();
	restartRobot = 1;
	runRobot = 0;
	releaseRobotThreadMutex();
}

DWORD WINAPI runRobotThread(LPVOID lpParam)
{
	while (true)
	{
		waitForRobotThreadMutex();
		if (restartRobot)
		{
			std::cout << "Restarting robot.." << std::endl;
			Robot::getInstance().getEngine().stop();
			VirtualHardwareController::getHardwareController().resetHardwareController();
			Navigator::getInstance().restart();
			Robot::getInstance().changeState(new FollowColorLaneState());
			restartRobot = 0;
			std::cout << "Restarted robot" << std::endl;
		}
		else if (runRobot)
		{
			Robot::getInstance().update();
		}
		releaseRobotThreadMutex();
		Sleep(5);
	}
	return 0;
}


VirtualHardwareController& VirtualHardwareController::getHardwareController()
{
	static VirtualHardwareController hwController;
	return hwController;
}

VirtualHardwareController::VirtualHardwareController() :
		colorlaneLeftSignal(false),
		colorlaneRightSignal(false),
		crossroadsSignal(false),
		collisionBackSignal(false),
		collisionFrontSignal(false),
		distanceSignal(false),
		emergencySignal(false),
		sideExitLeftSignal(false),
		sideExitRightSignal(false),
		colorlaneLeftToggle(true),
		colorlaneRightToggle(true),
		crossroadsToggle(false),
		collisionBackToggle(false),
		collisionFrontToggle(false),
		distanceToggle(false),
		emergencyToggle(false),
		sideExitLeftToggle(true),
		sideExitRightToggle(true),
		buzzer(false),
		led1(false),
		led2(false),
		wheelControl(true),
		sleep(false),
		leftWheel(Protocol::FORWARD),
		rightWheel(Protocol::FORWARD)
{
	//Start server
	SimulatorServer::startServer();
	//Start robot
	DWORD robotThreadId;
	HANDLE robotThreadHandle = CreateThread(NULL, 0, runRobotThread, NULL,
			0, &robotThreadId);
	if (robotThreadHandle == NULL)
	{
		std::cout << "Could not start robot thread: CreateThread() failed: " << GetLastError() << std::endl;
		//printf("Could not start client listen thread: CreateThread() failed: %d\n", GetLastError());
		return;
	}
	CloseHandle(robotThreadHandle);
}

void VirtualHardwareController::resetHardwareController()
{
	//Reset hardware controller
	waitForMutex();
	colorlaneLeftSignal = 0;
	colorlaneRightSignal = 0;
	crossroadsSignal = 0;
	collisionBackSignal = 0;
	collisionFrontSignal = 0;
	sideExitLeftSignal = 0;
	sideExitRightSignal = 0;
	distanceSignal = 0;
	emergencySignal = 0;
	colorlaneLeftToggle = 1;
	colorlaneRightToggle = 1;
	crossroadsToggle = 0;
	collisionBackToggle = 0;
	collisionFrontToggle = 0;
	distanceToggle = 0;
	emergencyToggle = 0;
	sideExitLeftToggle = 1;
	sideExitRightToggle = 1;
	buzzer = 0;
	led1 = 0;
	led2 = 0;
	wheelControl = 0;
	sleep = 0;
	leftWheel = Protocol::STOPPED;
	rightWheel = Protocol::STOPPED;
	releaseMutex();
	updateClients();
}

VirtualHardwareController::~VirtualHardwareController() { }

bool VirtualHardwareController::pollColorlaneLeft()
{
	waitForMutex();
	bool pollResult = colorlaneLeftToggle || colorlaneLeftSignal;
	colorlaneLeftSignal = 0;
	releaseMutex();
	return pollResult;
}

bool VirtualHardwareController::pollColorlaneRight()
{
	waitForMutex();
	bool pollResult = colorlaneRightToggle || colorlaneRightSignal;
	colorlaneRightSignal = 0;
	releaseMutex();
	return pollResult;
}

bool VirtualHardwareController::pollSideExitLeft()
{
	waitForMutex();
	bool pollResult = sideExitLeftToggle || sideExitLeftSignal;
	sideExitLeftSignal = 0;
	releaseMutex();
	return pollResult;
}

bool VirtualHardwareController::pollSideExitRight()
{
	waitForMutex();
	bool pollResult = sideExitRightToggle || sideExitRightSignal;
	sideExitRightSignal = 0;
	releaseMutex();
	return pollResult;
}

bool VirtualHardwareController::pollCrossRoadsSensor()
{
	waitForMutex();
	bool pollResult = crossroadsToggle || crossroadsSignal;
	crossroadsSignal = 0;
	releaseMutex();
	return pollResult;
}


bool VirtualHardwareController::pollCollisionSensorBack()
{
	waitForMutex();
	bool pollResult = collisionBackToggle || collisionBackSignal;
	collisionBackSignal = 0;
	releaseMutex();
	return pollResult;
}

bool VirtualHardwareController::pollCollisionSensorFront()
{
	waitForMutex();
	bool pollResult = collisionFrontToggle || collisionFrontSignal;
	collisionFrontSignal = 0;
	releaseMutex();
	return pollResult;
}

bool VirtualHardwareController::pollDistanceSensor()
{
	waitForMutex();
	bool pollResult = distanceToggle || distanceSignal;
	distanceSignal = 0;
	releaseMutex();
	return pollResult;
}

bool VirtualHardwareController::pollEmergencyStop()
{
	waitForMutex();
	bool pollResult = emergencyToggle || emergencySignal;
	emergencySignal = 0;
	releaseMutex();
	return pollResult;
}

void VirtualHardwareController::activateWheels()
{
	waitForMutex();
	wheelControl = true;
	SimulatorServer::sendUpdate(Protocol::WHEELCONTROL, 1);
	SimulatorServer::sendUpdate(Protocol::LEFTWHEEL, leftWheel);
	SimulatorServer::sendUpdate(Protocol::RIGHTWHEEL, rightWheel);
	releaseMutex();
}

void VirtualHardwareController::disableWheels()
{
	waitForMutex();
	wheelControl = false;
	SimulatorServer::sendUpdate(Protocol::WHEELCONTROL, 0);
	SimulatorServer::sendUpdate(Protocol::LEFTWHEEL, Protocol::STOPPED);
	SimulatorServer::sendUpdate(Protocol::RIGHTWHEEL, Protocol::STOPPED);
	releaseMutex();
}

void VirtualHardwareController::activateLED()
{
	waitForMutex();
	led1 = true;
	SimulatorServer::sendUpdate(Protocol::LED1, 1);
	releaseMutex();
}

void VirtualHardwareController::disableLED()
{
	waitForMutex();
	led1 = false;
	SimulatorServer::sendUpdate(Protocol::LED1, 0);
	releaseMutex();
}

void VirtualHardwareController::leftWheelForward()
{
	waitForMutex();
	leftWheel = Protocol::FORWARD;
	SimulatorServer::sendUpdate(Protocol::LEFTWHEEL, Protocol::FORWARD);
	releaseMutex();
}

void VirtualHardwareController::leftWheelBackward()
{
	waitForMutex();
	leftWheel = Protocol::BACKWARD;
	SimulatorServer::sendUpdate(Protocol::LEFTWHEEL, Protocol::BACKWARD);
	releaseMutex();
}

void VirtualHardwareController::rightWheelForward()
{
	waitForMutex();
	rightWheel = Protocol::FORWARD;
	SimulatorServer::sendUpdate(Protocol::RIGHTWHEEL, Protocol::FORWARD);
	releaseMutex();
}

void VirtualHardwareController::rightWheelBackward()
{
	waitForMutex();
	rightWheel = Protocol::BACKWARD;
	SimulatorServer::sendUpdate(Protocol::RIGHTWHEEL, Protocol::BACKWARD);
	releaseMutex();
}

void VirtualHardwareController::wait(unsigned int ms)
{
	waitForMutex();
	sleep = 1;
//	SimulatorServer::sendUpdate(Protocol::SLEEP, sleep);
	releaseMutex();
	Sleep(ms);
	waitForMutex();
	sleep = 0;
//	SimulatorServer::sendUpdate(Protocol::SLEEP, sleep);
	releaseMutex();
}

void VirtualHardwareController::wait(unsigned int* ms)
{
	waitForMutex();
	sleep = 1;
//	SimulatorServer::sendUpdate(Protocol::SLEEP, sleep);
	releaseMutex();
	Sleep(*ms);
	(*ms) = 0;
	waitForMutex();
	sleep = 0;
//	SimulatorServer::sendUpdate(Protocol::SLEEP, sleep);
	releaseMutex();
}

void VirtualHardwareController::processTrigger(Protocol::Trigger trigger)
{
	bool triggerValue = (trigger.value) ? 1 : 0;
	waitForMutex();
	switch (trigger.type)
	{
	//Signals
	case Protocol::EMERGENCYSIGNAL:
		emergencySignal = triggerValue;
		break;
	case Protocol::COLLISIONBACKSIGNAL:
		collisionBackSignal = triggerValue;
		break;
	case Protocol::COLLISIONFRONTSIGNAL:
		collisionFrontSignal = triggerValue;
		break;
	case Protocol::PROXIMITYSIGNAL:
		distanceSignal = triggerValue;
		break;
	case Protocol::COLORLANELEFTSIGNAL:
		colorlaneLeftSignal = triggerValue;
		break;
	case Protocol::COLORLANERIGHTSIGNAL:
		colorlaneRightSignal = triggerValue;
		break;
	case Protocol::CROSSROADSSIGNAL:
		crossroadsSignal = triggerValue;
		break;
	case Protocol::SIDEXITLEFTSIGNAL:
		sideExitLeftSignal = triggerValue;
		break;
	case Protocol::SIDEEXITRIGHTSIGNAL:
		sideExitRightSignal = triggerValue;
		break;

	//Toggles
	case Protocol::EMERGENCYTOGGLE:
		emergencyToggle = !emergencyToggle;
		break;
	case Protocol::COLLISIONBACKTOGGLE:
		collisionBackToggle = !collisionBackToggle;
		break;
	case Protocol::COLLISIONFRONTTOGGLE:
		collisionFrontToggle = ! collisionFrontToggle;
		break;
	case Protocol::PROXIMITYTOGGLE:
		distanceToggle = !distanceToggle;
		break;
	case Protocol::COLORLANELEFTTOGGLE:
		colorlaneLeftToggle = !colorlaneLeftToggle;
		SimulatorServer::sendUpdate(Protocol::COLORLANELEFTSENSOR, colorlaneLeftSignal || colorlaneLeftToggle);
		break;
	case Protocol::COLORLANERIGHTTOGGLE:
		colorlaneRightToggle = !colorlaneRightToggle;
		SimulatorServer::sendUpdate(Protocol::COLORLANERIGHTSENSOR, colorlaneRightSignal || colorlaneRightToggle);
		break;
	case Protocol::CROSSROADSTOGGLE:
		crossroadsToggle = !crossroadsToggle;
		SimulatorServer::sendUpdate(Protocol::CROSSROADSSENSOR, crossroadsSignal || crossroadsToggle);
		break;
	case Protocol::SIDEEXITLEFTTOGGLE:
		sideExitLeftToggle = !sideExitLeftToggle;
		SimulatorServer::sendUpdate(Protocol::SIDEEXITLEFTSENSOR, sideExitLeftSignal || sideExitLeftToggle);
		break;
	case Protocol::SIDEEXITRIGHTTOGGLE:
		sideExitRightToggle = !sideExitRightToggle;
		SimulatorServer::sendUpdate(Protocol::SIDEEXITRIGHTSENSOR, sideExitRightSignal || sideExitRightToggle);
		break;

		//(Server) commands
	case Protocol::START:
		releaseMutex();
		startRobotThread();
		return;
	case Protocol::PAUZE:
		releaseMutex();
		pauzeRobotThread();
		return;
	case Protocol::RESTART:
		releaseMutex();
		restartRobotThread();
		return;
	}
	releaseMutex();
}

void VirtualHardwareController::updateClient(unsigned int clientID)
{
	waitForMutex();
	SimulatorServer::sendUpdate(Protocol::BUZZER, buzzer, clientID);
	SimulatorServer::sendUpdate(Protocol::LED1, led1, clientID);
	SimulatorServer::sendUpdate(Protocol::LED2, led2, clientID);
	SimulatorServer::sendUpdate(Protocol::WHEELCONTROL, wheelControl, clientID);
	SimulatorServer::sendUpdate(Protocol::LEFTWHEEL, leftWheel, clientID);
	SimulatorServer::sendUpdate(Protocol::RIGHTWHEEL, rightWheel, clientID);
	SimulatorServer::sendUpdate(Protocol::SLEEP, sleep, clientID);
	SimulatorServer::sendUpdate(Protocol::COLORLANELEFTSENSOR, colorlaneLeftSignal || colorlaneLeftToggle, clientID);
	SimulatorServer::sendUpdate(Protocol::COLORLANERIGHTSENSOR, colorlaneRightSignal || colorlaneRightToggle, clientID);
	SimulatorServer::sendUpdate(Protocol::CROSSROADSSENSOR, crossroadsSignal || crossroadsToggle, clientID);
	SimulatorServer::sendUpdate(Protocol::SIDEEXITLEFTSENSOR, sideExitLeftSignal || sideExitLeftToggle, clientID);
	SimulatorServer::sendUpdate(Protocol::SIDEEXITRIGHTSENSOR, sideExitRightSignal || sideExitRightToggle, clientID);
	releaseMutex();
}

void VirtualHardwareController::updateClients()
{
	waitForMutex();
	SimulatorServer::sendUpdate(Protocol::BUZZER, buzzer);
	SimulatorServer::sendUpdate(Protocol::LED1, led1);
	SimulatorServer::sendUpdate(Protocol::LED2, led2);
	SimulatorServer::sendUpdate(Protocol::WHEELCONTROL, wheelControl);
	SimulatorServer::sendUpdate(Protocol::LEFTWHEEL, leftWheel);
	SimulatorServer::sendUpdate(Protocol::RIGHTWHEEL, rightWheel);
	SimulatorServer::sendUpdate(Protocol::SLEEP, sleep);
	SimulatorServer::sendUpdate(Protocol::COLORLANELEFTSENSOR, colorlaneLeftSignal || colorlaneLeftToggle);
	SimulatorServer::sendUpdate(Protocol::COLORLANERIGHTSENSOR, colorlaneRightSignal || colorlaneRightToggle);
	SimulatorServer::sendUpdate(Protocol::CROSSROADSSENSOR, crossroadsSignal || crossroadsToggle);
	SimulatorServer::sendUpdate(Protocol::CROSSROADSSENSOR, crossroadsSignal || crossroadsToggle);
	SimulatorServer::sendUpdate(Protocol::SIDEEXITLEFTSENSOR, sideExitLeftSignal || sideExitLeftToggle);
	SimulatorServer::sendUpdate(Protocol::SIDEEXITRIGHTSENSOR, sideExitRightSignal || sideExitRightToggle);
	releaseMutex();
}
